﻿#include "ForceCtrDLL.h"

int main()
{
    FRRobot robot;
    std::string rcvStr;
    int rcvInt;
    FR10IdentificationResult result;
    
    if (!robot.Connect("192.168.58.2", 8080)) {
        std::cerr << "使用 C++ 接口连接失败！\n";
        return -1;
    }
    std::cerr << "使用 C++ 接口成功连接！";
    int choice = -1;
    while (choice != 0) {
        std::cout << "\n请选择要执行的指令功能：" << std::endl;
        std::cout << "1. 末端负载全部清零" << std::endl;
        std::cout << "2. 寻找标定点位" << std::endl;
        std::cout << "3. 传感器零点标定" << std::endl;
        std::cout << "4. FR10负载辨识" << std::endl;
        std::cout << "5. FR10力辅助拖动" << std::endl;
        std::cout << "0. 退出" << std::endl;
        std::cout << "请输入选项: ";
        std::cin >> choice;
        std::cin.ignore(); // 清除输入缓冲区中的换行符

        switch (choice) {
        case 1:
            // 末端负载全部清零
            rcvInt = robot.ClearEndEffectorLoad();
            std::cerr << rcvInt << std::endl;
            break;
        case 2:
            // 寻找标定点位
            rcvInt = robot.GetZeroConfiguration();
            std::cerr << rcvInt << std::endl;
            break;
        case 3:
            // 传感器零点标定
            rcvInt = robot.StartSensorZerosCalibration();
            std::cerr << rcvInt << std::endl;
            break;
        case 4:
            // FR10负载辨识
            if (robot.FR10LoadIdentification(&result) == 0) {
                std::cout << "负载重量辨识结果: " << result.payloadWeight << std::endl;
                std::cout << "负载重心辨识结果: " << result.payloadCOG << std::endl;
            }
            else {
                std::cerr << "负载辨识失败！" << std::endl;
            }
            break;
        case 5:
            // 力辅助拖动
            int control_time;
            double sensitivity;
            std::cout << "请输入控制时间（秒）: ";
            std::cin >> control_time; // 用户输入控制时间
            std::cout << "请输入灵敏度（0精拖动-1粗拖动）: ";
            std::cin >> sensitivity; // 用户输入拖动灵敏度
            rcvInt = robot.ForceCtrDragAdaptiveControl(sensitivity);
            if (rcvInt == 0)
            {
                std::cerr << "力辅助拖动开启成功" << std::endl;
            }
            else
            {
                std::cerr << "力辅助拖动开启失败，错误码：" << rcvInt << std::endl;
                break;
            }
            std::cerr << "力辅助拖动开启中，时间：" << control_time << "秒" << std::endl;
            std::this_thread::sleep_for(std::chrono::seconds(control_time)); // 使用用户输入的时间
            rcvInt = robot.CloseForceCtrDrag();
            std::cerr << rcvInt << std::endl;
            break;
        case 0:
            std::cout << "退出程序。" << std::endl;
            break;
        default:
            std::cout << "无效选择，请重新输入。" << std::endl;
            break;
        }
    }
}

